clc; clear; close all;
load("aerodata.mat")
global Init_Data
Init_Data = pp;
% num = 1;
% Init_Data(num) = 0.3;num=num+1;
% Init_Data(num) = 0.01;num=num+1;Init_Data(num) = 0.05;num=num+1;Init_Data(num) = 0.05;num=num+1;
% Init_Data(num) = 0.01;num=num+1;Init_Data(num) = 0.00;num=num+1;Init_Data(num) = 0.25;num=num+1;Init_Data(num) = 0.00;num=num+1;Init_Data(num) = 0.25;num=num+1;
% Init_Data(num) = 0.025^2;num=num+1;Init_Data(num) = 9.81;num=num+1;Init_Data(num) = 1.4;num=num+1;
% Init_Data(num) = -0.05;num=num+1;Init_Data(num) = 0;num=num+1;Init_Data(num) = 0;num=num+1;
% Init_Data(num) = 0.1;num=num+1;Init_Data(num) = 0;num=num+1;Init_Data(num) = 0;num=num+1;
%% 目标点
target = [MyShootMethod_Constants.x_T, MyShootMethod_Constants.y_T, MyShootMethod_Constants.z_T]; % 目标点 (x_T, y_T, z_T)

%% 迭代求解初始速度
v0_initial = [15*cosd(45), 15*sind(45), 0]; % 初始猜测速度 (vx0, vy0, vz0)
res = simulate(v0_initial);
v0_solution = update_initial_velocity(v0_initial, target);

fprintf('命中目标所需初始速度: vx0 = %.2f, vy0 = %.2f, vz0 = %.2f\n', v0_solution(1), v0_solution(2), v0_solution(3));

%% 函数部分

% 计算误差
function error = compute_error(final_state, target)
    xf = final_state(1);
    yf = final_state(2);
    zf = final_state(3);
    xt = target(1);
    yt = target(2);
    zt = target(3);
    error = [xf - xt, yf - yt, zf - zt]; % 计算误差
end

% % 运动学方程 (待补充 f_x, f_y, f_z)
% function dydt = dynamics(state)
%     x = state(1);
%     y = state(2);
%     z = state(3);
%     vx = state(4);
%     vy = state(5);
%     vz = state(6);
    
%     fx = 0;  % TODO: 填写 x 方向的加速度
%     fy = 0;  % TODO: 填写 y 方向的加速度
%     fz = -9.81; % 仅考虑重力

%     dydt = [vx; vy; vz; fx; fy; fz];
% end

% % Runge-Kutta 4 方法
% function state_new = runge_kutta4(state, dt)
%     k1 = dynamics(state);
%     k2 = dynamics(state + 0.5 * dt * k1);
%     k3 = dynamics(state + 0.5 * dt * k2);
%     k4 = dynamics(state + dt * k3);
%     state_new = state + (dt / 6) * (k1 + 2*k2 + 2*k3 + k4);
% end
function [value, isterminal, direction] = stop_cond(t, y)
    value = y(2) - MyShootMethod_Constants.y_T; % 当 y(2) 小于 0.5 时触发事件
    isterminal = 1; % 触发事件后终止求解
    direction = -1; % 穿过
end
% 轨迹模拟
function final_state = simulate(v0)
    dt = 0.005;
    t_max = 10;
    state = [0, 0, 0, v0(1), v0(2), v0(3),...
    my_angle2quat([atan2(v0(2),v0(1)),-atan2(v0(3),sqrt(v0(1)^2+v0(2)^2)),0].').',...
    0,0,0]; % 初始状态 [x, y, z, vx, vy, vz]
    %t = 0;
    
    %stop_cond = @(t, y) y(2,1) < 0.5 && t>1;
    %[~,y]=rk4_fixed_step_stop(@dynamic_equation,[0 t_max],state,dt,stop_cond);
    options = odeset('Events',@stop_cond);
    [~,y]=ode45(@dynamic_equation,[0 t_max],state,options);
    final_state = y(end,:);
    %final_state = state;
end

% 使用有限差分计算梯度
function gradient = finite_difference_gradient(v0, target, delta)
    if nargin < 3
        delta = 1e-3; % 设定微小扰动量
    end
    %error = zeros(1,3);
    gradient = zeros(1,3);
    
    for i = 1:3  % 只对 vx0, vy0, vz0 求偏导数
        v0_perturbed = v0;
        v0_perturbed(i) = v0_perturbed(i) + delta;
        
        final_state_perturbed = simulate(v0_perturbed);
        error_perturbed = compute_error(final_state_perturbed, target);
        
        final_state = simulate(v0);
        error = compute_error(final_state, target);
        
        gradient(i) = (error_perturbed(i) - error(i)) / delta;
    end
end

% 梯度下降法迭代求解初始速度
function v0 = update_initial_velocity(v0, target, alpha, tol, max_iter)
    if nargin < 3
        alpha = 0.01; % 学习率
    end
    if nargin < 4
        tol = 1e-3; % 误差阈值
    end
    if nargin < 5
        max_iter = 1000; % 最大迭代次数
    end
    
    for iter = 1:max_iter
        final_state = simulate(v0);
        error = compute_error(final_state, target);

        if norm(error) < tol  % 误差足够小，停止迭代
            break;
        end
        
        gradient = finite_difference_gradient(v0, target);

        if mod(iter,10) == 0
            fprintf('*************iteration:%d***************\n',iter)
            fprintf('当前状态: xf = %.4f, yf = %.4f, zf = %.4f\n', final_state(1), final_state(2), final_state(3));
            fprintf('迭代前速度: vx0 = %.4f, vy0 = %.4f, vz0 = %.4f\n', v0(1), v0(2), v0(3));
            fprintf('迭代前误差: x_err = %.4f, y_err = %.4f, z_err = %.4f,norm_err = %.4f\n', error(1), error(2), error(3),norm(error));
            fprintf('梯度: gradient_vx = %.4f, gradient_vy = %.4f, gradient_vz = %.4f\n', gradient(1), gradient(2), gradient(3));
            fprintf('****************************************\n\n\n\n')    
        end
        
        v0 = v0 - alpha * gradient .* error; % 迭代更新速度
    end
    if(iter == max_iter)
        fprintf('达到最大迭代次数\n');
    end
end